#include <ros/ros.h>
#include <ros/package.h>
// ROSBag
#include <rosbag/bag.h>
#include <rosbag/view.h>

// Boost
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>

#include <math.h>
#include <iostream>
#include <ctype.h>
#include <fstream>

//KDL related
#include <kdl/chain.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames.hpp>
#include <kdl/jacobian.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/jntspaceinertiamatrix.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/tree.hpp>

#include <sensor_msgs/JointState.h>

using namespace std;

void extract_key_frame(std::string trajectory_file,
                       std::vector< std::vector<double> > &joint_position);

int main(int argc, char **argv)
{
	ros::init(argc, argv, "jaco_bag_read_j2c");
	ros::NodeHandle nh;
	//get robot state
	std::cout << "Getting the urdf..." << std::endl;
	std::string robot_description;
	std::string robot_param;

	nh.searchParam("robot_description", robot_param);
	nh.param<std::string>(robot_param, robot_description, "");

	//check the param server return
	if (robot_description.empty())
	{
		std::cout << "Failed to retreive robot urdf" << std::endl;
		return 0;
	}

	//parse the KDL tree
	KDL::Tree kdl_tree;
	if (!kdl_parser::treeFromString(robot_description, kdl_tree))
	{
		std::cout << "Failed to parse the kdl tree" << std::endl;
		return 0;
	}

	//parse the KDL chain
	KDL::Chain jaco_chain;
	std::string arm_name_ = "jaco";
	//nh.param("wpi_jaco/arm_name", arm_name_, std::string("jaco"));
	if (!kdl_tree.getChain(arm_name_ + "_link_base", arm_name_ + "_link_hand", jaco_chain))
	{
		std::cout << "Failed to parse the kdl chain" << std::endl;
		return 0;
	}

	boost::shared_ptr<KDL::Chain> kdl_chain_ptr = boost::make_shared<KDL::Chain>(jaco_chain);
	std::cout << "KDL chain has " << kdl_chain_ptr->getNrOfSegments() << " segments and " << kdl_chain_ptr->getNrOfJoints() << " joints." << std::endl;

	std::cout << "Joints: ";
	for (unsigned int i = 0; i < kdl_chain_ptr->getNrOfSegments(); i++)
		std::cout << kdl_chain_ptr->segments.at(i).getJoint().getName() << " ";
	std::cout << std::endl;

	boost::shared_ptr<KDL::ChainFkSolverPos>    jnt_to_pose_solver_;
	boost::shared_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
	jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(jaco_chain));
	jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(jaco_chain));


	//--------------------------------Read data from demonstration----------------------------------//
	//std::string package_path_ = ros::package::getPath( "jaco_ik" ) + "/";
	std::string folder_name = "/home/hyperhu/apc2016_arm/20170517-motion-pouring water/far/";
	std::string bag_name = "4_2017-05-17-16-06-07.bag";
	std::string trajectory_file = folder_name + bag_name;
	std::vector< std::vector<double> > joint_position;
	std::vector< std::vector<double> > cartesian_position;
	extract_key_frame(trajectory_file, joint_position);

	for (int i = 0; i < joint_position.size(); i++)
	{
		KDL::JntArray  q_KDL;
		q_KDL.resize(6);
		KDL::Frame     x_KDL;
		std::vector<double> x_VEC;
		std::cout << "fuck 1";
		for (unsigned int j = 0; j < 6; j++)
		{
			q_KDL(j) = joint_position[i][j];
		}
		jnt_to_pose_solver_->JntToCart(q_KDL, x_KDL);
		std::cout << "fuck 2";
		x_VEC.push_back( x_KDL.p.x() );
		x_VEC.push_back( x_KDL.p.y() );
		x_VEC.push_back( x_KDL.p.z() );
		double x, y, z, w;
		x_KDL.M.GetQuaternion(x, y, z, w);
		x_VEC.push_back(x);
		x_VEC.push_back(y);
		x_VEC.push_back(z);
		x_VEC.push_back(w);
		cartesian_position.push_back(x_VEC);
	}


	std::string dir_name;
	dir_name = "";
	for (string::size_type i = 0; i < bag_name.size(); i++)
	{
		// if (!isalnum(bag_name[i]))
		if (bag_name[i] == '.')
			break;
		dir_name += bag_name[i];
	}
	ROS_INFO_STREAM(dir_name);

	std::string outfile_position = folder_name + dir_name + "_cartesian_position.txt";
	std::ofstream outfile_p(outfile_position.c_str());

	if (!outfile_p)
	{
		std::cout << "Unable to open outfile";
		exit(1); // terminate with error
	}
	for (unsigned int i = 0; i < cartesian_position.size(); i++)
	{
		outfile_p << cartesian_position[i][0] << "|"  << cartesian_position[i][1] << "|" << cartesian_position[i][2] << "|"
		          << cartesian_position[i][3] << "|" << cartesian_position[i][4] << "|" << cartesian_position[i][5] << "|" << cartesian_position[i][6] << "|" << std::endl;
	}
	outfile_p.close();

}

void extract_key_frame(std::string trajectory_file,
                       std::vector< std::vector<double> > &joint_position)
{
	rosbag::Bag trajectory_bag;
	std::vector<sensor_msgs::JointState> demo_c_states;
	if (boost::filesystem::exists(trajectory_file))
	{
		//ROS_INFO_STREAM("Loading trajectory from file.");
		trajectory_bag.open(trajectory_file, rosbag::bagmode::Read);

		std::vector<std::string> topics;

		// Get the forward trajectory
		topics.clear();
		topics.push_back(std::string("/jaco_arm/joint_states"));
		rosbag::View view(trajectory_bag, rosbag::TopicQuery(topics));

		BOOST_FOREACH(rosbag::MessageInstance const msg_instance, view)
		{
			//ROS_INFO_STREAM("Read data for correspond topic");
			sensor_msgs::JointState::ConstPtr c_state = msg_instance.instantiate<sensor_msgs::JointState>();
			if (c_state != NULL)
			{
				demo_c_states.push_back(*c_state);
			}
			else
			{
				ROS_INFO_STREAM("error read data from bag file");
			}
		}
	}
	else
	{
		ROS_ERROR_STREAM("No File Exists");
	}
	trajectory_bag.close();
	ROS_INFO_STREAM(demo_c_states.size());

	for (int i = 0; i < demo_c_states.size(); i++)
	{
		joint_position.push_back(demo_c_states[i].position);
	}

}